package com.rockwell.maze800.views;

import java.text.DecimalFormat;
import java.util.List;

import org.opencv.android.BaseLoaderCallback;
import org.opencv.android.CameraBridgeViewBase.CvCameraViewFrame;
import org.opencv.android.LoaderCallbackInterface;
import org.opencv.android.OpenCVLoader;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.android.CameraBridgeViewBase;
import org.opencv.android.CameraBridgeViewBase.CvCameraViewListener2;
import org.opencv.imgproc.Imgproc;

import com.rockwell.maze800.R;
import com.rockwell.maze800.SettingsActivity;
import com.rockwell.maze800.model.ColorBlobDetector;
import com.rockwell.maze800.model.ColorBlobInfo;

import android.content.Context;
import android.content.SharedPreferences;
import android.content.SharedPreferences.Editor;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.preference.PreferenceManager;
import android.util.AttributeSet;
import android.util.Log;
import android.view.LayoutInflater;
import android.view.MotionEvent;
import android.view.View;
import android.view.View.OnTouchListener;
import android.widget.RelativeLayout;
import android.widget.TextView;

enum TEACHMODE
{
	TEACH_TREATURE,
	TEACH_BARRIER,
	TEACH_TRASH
}

public class MazeCameraView extends RelativeLayout implements OnTouchListener, CvCameraViewListener2, SensorEventListener {

	private static final String  TAG = "MazeCameraView::RelativeLayout";

	private OnOrientationChangeListener m_OrientationListener;
	private OnColorBlobDetectListener 	m_TreasureListener;
	private OnColorBlobDetectListener 	m_BarrierListener;
	private OnColorBlobDetectListener 	m_TrashListener;
	
	private boolean				 m_TeachMode = false;
    private boolean              m_IsColorSelected = false;
    private Mat                  m_Rgba;
    private Scalar               m_BlobColorRgba;
    private Scalar               m_BlobColorHsv;
    private ColorBlobDetector    m_TreasureColorDetector;
    private ColorBlobDetector    m_TrashColorDetector;
    private ColorBlobDetector    m_BarrierColorDetector;
    private Mat                  m_Spectrum;
    private Size                 SPECTRUM_SIZE;
    private Scalar               CONTOUR_COLOR;
    
    private CameraBridgeViewBase m_OpenCvCameraView;
    private TextView 			 m_CompassValueView;
    
    // Compass parameters
    private SensorManager 		 m_SensorManager;
    private Sensor 				 m_Gsensor;
    private Sensor 				 m_Msensor;
    private float[] 			 mGravity = new float[3];
    private float[] 			 mGeomagnetic = new float[3];
    private float 				 azimuth = 0f;
    private float 				 currectAzimuth = 0;
    private float[]				 m_Orientation = new float[3];
    private int 				 m_Count;
    private TEACHMODE			 m_teachIndex = TEACHMODE.TEACH_TREATURE;

    private Point m_treasureCenter = new Point();
    private Float m_treasureRadius = new Float(0);
    
    private Point m_barrierCenter = new Point();
    private Float m_barrierRadius = new Float(0);
    
    private Point m_trashCenter = new Point();
    private Float m_trashRadius = new Float(0);
    
    private BaseLoaderCallback  m_LoaderCallback = new BaseLoaderCallback(this.getContext()) {
        @Override
        public void onManagerConnected(int status) {
            switch (status) {
                case LoaderCallbackInterface.SUCCESS:
                {
                    Log.i(TAG, "OpenCV loaded successfully");
                    m_OpenCvCameraView.enableView();
                } 
                break;
                default:
                {
                    super.onManagerConnected(status);
                } 
                break;
            }
        }
    };

	public MazeCameraView(Context context) {
		super(context);
		init(context);
	}
	
	public MazeCameraView(Context context, AttributeSet attrs) {
		super(context, attrs);
		init(context);
	}
	
	public MazeCameraView(Context context, AttributeSet attrs, int defStyle) {
		super(context, attrs, defStyle);
		init(context);
	}
	
	public void setOrientationChangeListener(OnOrientationChangeListener l) {
		m_OrientationListener = l;
	}
	
	public void setTreasureDetectListener(OnColorBlobDetectListener l) {
		m_TreasureListener = l;
	}
	
	public void setBarrierDetectListener(OnColorBlobDetectListener l) {
		m_BarrierListener = l;
	}
	
	public void setTrashDetectListener(OnColorBlobDetectListener l) {
		m_TrashListener = l;
	}

	@Override
	protected void onLayout(boolean arg0, int arg1, int arg2, int arg3, int arg4) {
		super.onLayout(arg0, arg1, arg2, arg3, arg4);
	}

	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
	}

	@Override
    public void onSensorChanged(SensorEvent event) {
        final float alpha = 0.97f;

        synchronized (this) {
            if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
                    mGravity[0] = alpha * mGravity[0] + (1 - alpha)
                                    * event.values[0];
                    mGravity[1] = alpha * mGravity[1] + (1 - alpha)
                                    * event.values[1];
                    mGravity[2] = alpha * mGravity[2] + (1 - alpha)
                                    * event.values[2];
            }

            if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
                    mGeomagnetic[0] = alpha * mGeomagnetic[0] + (1 - alpha)
                                    * event.values[0];
                    mGeomagnetic[1] = alpha * mGeomagnetic[1] + (1 - alpha)
                                    * event.values[1];
                    mGeomagnetic[2] = alpha * mGeomagnetic[2] + (1 - alpha)
                                    * event.values[2];
            }

            float R[] = new float[9];
            float I[] = new float[9];
            boolean success = SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
            
            if (success) {
        		float[] data = new float[3];
                SensorManager.getOrientation(R, data);

                data[0] = (float) Math.toDegrees(data[0]);
                data[0] = (data[0] + 360) % 360;
                data[1] = (float)Math.toDegrees(data[1]);
	        	data[2] = (float)Math.toDegrees(data[2]);
	        	
	        	System.arraycopy(data, 0, m_Orientation, 0, 3);
	        	
	        	DecimalFormat displayFormat = new DecimalFormat("#.#");
                
                String sValues = "Azimuth:\t" + String.valueOf(displayFormat.format(m_Orientation[0])) +
                         		 "\nPitch:\t" + String.valueOf(displayFormat.format(m_Orientation[1])) +
                         		 "\nRoll:\t" + String.valueOf(displayFormat.format(m_Orientation[2]));

	            Log.d("Compass", sValues);
	            m_CompassValueView.setText(sValues);
	            
	            if (m_OrientationListener != null)
	            	m_OrientationListener.onOrientationChanged(m_Orientation);
            }
        }
    }

	@Override
	public void onCameraViewStarted(int width, int height) {
        m_Rgba = new Mat(height, width, CvType.CV_8UC4);
        m_TreasureColorDetector = new ColorBlobDetector(new Scalar(0, 255, 0));
        m_TrashColorDetector = new ColorBlobDetector(new Scalar(0, 0, 255));
        m_BarrierColorDetector = new ColorBlobDetector(new Scalar(255, 0, 0));
        m_Spectrum = new Mat();
        m_BlobColorRgba = new Scalar(255);
        m_BlobColorHsv = new Scalar(255);
        SPECTRUM_SIZE = new Size(200, 64);
        CONTOUR_COLOR = new Scalar(255,0,0,255);
        
        getTreasureColor();
        getTrashColor();
        getBarrierColor();
        
        m_IsColorSelected = true;
    }

	@Override
	public void onCameraViewStopped() {
		m_Rgba.release();
	}

	@Override
	public boolean onTouch(View v, MotionEvent event) {
        int cols = m_Rgba.cols();
        int rows = m_Rgba.rows();

        int xOffset = (m_OpenCvCameraView.getWidth() - cols) / 2;
        int yOffset = (m_OpenCvCameraView.getHeight() - rows) / 2;

        int x = (int)event.getX() - xOffset;
        int y = (int)event.getY() - yOffset;

        Log.i(TAG, "Touch image coordinates: (" + x + ", " + y + ")");

        if ((x < 0) || (y < 0) || (x > cols) || (y > rows)) return false;

        Rect touchedRect = new Rect();

        touchedRect.x = (x>4) ? x-4 : 0;
        touchedRect.y = (y>4) ? y-4 : 0;

        touchedRect.width = (x+4 < cols) ? x + 4 - touchedRect.x : cols - touchedRect.x;
        touchedRect.height = (y+4 < rows) ? y + 4 - touchedRect.y : rows - touchedRect.y;

        Mat touchedRegionRgba = m_Rgba.submat(touchedRect);

        Mat touchedRegionHsv = new Mat();
        Imgproc.cvtColor(touchedRegionRgba, touchedRegionHsv, Imgproc.COLOR_RGB2HSV_FULL);

        // Calculate average color of touched region
        m_BlobColorHsv = Core.sumElems(touchedRegionHsv);
        int pointCount = touchedRect.width*touchedRect.height;
        for (int i = 0; i < m_BlobColorHsv.val.length; i++)
            m_BlobColorHsv.val[i] /= pointCount;

        if (m_teachIndex == TEACHMODE.TEACH_TREATURE) {
        	setTreasureColor();
        	m_teachIndex = TEACHMODE.TEACH_BARRIER;
        }
        else if (m_teachIndex == TEACHMODE.TEACH_BARRIER) {
        	setBarrierColor();
        	m_teachIndex = TEACHMODE.TEACH_TRASH;
        }
        else {
        	setTrashColor();
        	m_teachIndex = TEACHMODE.TEACH_TREATURE;
        }

        touchedRegionRgba.release();
        touchedRegionHsv.release();

        return false; // don't need subsequent touch events
    }
	
	private void setTreasureColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

        Log.i(TAG, "set treasure rgba color: (" + m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] +
                ", " + m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        m_TreasureColorDetector.setHsvColor(m_BlobColorHsv);

        Imgproc.resize(m_TreasureColorDetector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

        //m_IsColorSelected = true;
        
        SettingsActivity.setTreasureColor(getContext(), m_BlobColorHsv);
	}
	
	private void setTrashColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

        Log.i(TAG, "set trash rgba color: (" + m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] +
                ", " + m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        m_TrashColorDetector.setHsvColor(m_BlobColorHsv);

        Imgproc.resize(m_TrashColorDetector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

        //m_IsColorSelected = true;
        
        SettingsActivity.setTrashColor(getContext(), m_BlobColorHsv);
	}
	
	private void setBarrierColor() {
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

        Log.i(TAG, "set barrier rgba color: (" + m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] +
                ", " + m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        m_BarrierColorDetector.setHsvColor(m_BlobColorHsv);

       //Imgproc.resize(m_BarrierColorDetector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);

        m_IsColorSelected = true;
        
        SettingsActivity.setBarrierColor(getContext(), m_BlobColorHsv);
	}
	
	private void getTreasureColor() {
		m_BlobColorHsv = SettingsActivity.getTreasureColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

        Log.i(TAG, "get treasure rgba color: (" + m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] +
                ", " + m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        m_TreasureColorDetector.setHsvColor(m_BlobColorHsv);

        Imgproc.resize(m_TreasureColorDetector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);
	}
	
	private void getTrashColor() {
		m_BlobColorHsv = SettingsActivity.getTrashColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

        Log.i(TAG, "get trash rgba color: (" + m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] +
                ", " + m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        m_TrashColorDetector.setHsvColor(m_BlobColorHsv);

        Imgproc.resize(m_TrashColorDetector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);
	}
	
	private void getBarrierColor() {
		m_BlobColorHsv = SettingsActivity.getBarrierColor(getContext());
		m_BlobColorRgba = converScalarHsv2Rgba(m_BlobColorHsv);

        Log.i(TAG, "get barrier rgba color: (" + m_BlobColorRgba.val[0] + ", " + m_BlobColorRgba.val[1] +
                ", " + m_BlobColorRgba.val[2] + ", " + m_BlobColorRgba.val[3] + ")");

        m_BarrierColorDetector.setHsvColor(m_BlobColorHsv);

        //Imgproc.resize(m_BarrierColorDetector.getSpectrum(), m_Spectrum, SPECTRUM_SIZE);
	}
	
	private void detectTreasure() {
		m_TreasureColorDetector.process(m_Rgba);
        List<MatOfPoint> contours = m_TreasureColorDetector.getContours();
        Log.i(TAG, "Treasure Contours count: " + contours.size());
        Imgproc.drawContours(m_Rgba, contours, -1, CONTOUR_COLOR);
        
        ColorBlobInfo info = new ColorBlobInfo();
        boolean present = m_TreasureColorDetector.drawContourCenters(m_Rgba, info);
        
        if (m_TreasureListener != null)
        	m_TreasureListener.onDetected(present, info.center, info.radius, m_Rgba.height(), m_Rgba.width());
	}
	
	private void detectTrash() {
		m_TrashColorDetector.process(m_Rgba);
        List<MatOfPoint> contours = m_TrashColorDetector.getContours();
        Log.i(TAG, "Trash Contours count: " + contours.size());
        Imgproc.drawContours(m_Rgba, contours, -1, CONTOUR_COLOR);
        
        ColorBlobInfo info = new ColorBlobInfo();
        boolean present = m_TrashColorDetector.drawContourCenters(m_Rgba, info);
        
        if (m_TrashListener != null)
        	m_TrashListener.onDetected(present, info.center, info.radius, m_Rgba.height(), m_Rgba.width());
	}
	
	private void detectBarrier() {
		m_BarrierColorDetector.process(m_Rgba);
        List<MatOfPoint> contours = m_BarrierColorDetector.getContours();
        Log.i(TAG, "Barrier Contours count: " + contours.size());
        Imgproc.drawContours(m_Rgba, contours, -1, CONTOUR_COLOR);
        
        ColorBlobInfo info = new ColorBlobInfo();
        boolean present = m_BarrierColorDetector.drawContourCenters(m_Rgba, info);
        
        if (m_BarrierListener != null)
        	m_BarrierListener.onDetected(present, info.center, info.radius, m_Rgba.height(), m_Rgba.width());
	}
	
	public void setTeachMode(boolean teachMode) {
		m_TeachMode = teachMode;
		
		if (m_TeachMode)
        	m_OpenCvCameraView.setOnTouchListener(MazeCameraView.this);
		else
			m_OpenCvCameraView.setOnTouchListener(null);
	}
	
	public Mat onCameraFrame(CvCameraViewFrame inputFrame) {
        m_Rgba = inputFrame.rgba();

        if (m_IsColorSelected) {
        	detectTreasure();
        	detectBarrier();
        	detectTrash();

            Mat colorLabel = m_Rgba.submat(4, 68, 4, 68);
            colorLabel.setTo(m_BlobColorRgba);

            Mat spectrumLabel = m_Rgba.submat(4, 4 + m_Spectrum.rows(), 70, 70 + m_Spectrum.cols());
            m_Spectrum.copyTo(spectrumLabel);
        }

        return m_Rgba;
    }

    private Scalar converScalarHsv2Rgba(Scalar hsvColor) {
        Mat pointMatRgba = new Mat();
        Mat pointMatHsv = new Mat(1, 1, CvType.CV_8UC3, hsvColor);
        Imgproc.cvtColor(pointMatHsv, pointMatRgba, Imgproc.COLOR_HSV2RGB_FULL, 4);

        return new Scalar(pointMatRgba.get(0, 0));
    }
	
	private void init(Context context) {
		Log.i(TAG, "called init");
		
		LayoutInflater inflater = (LayoutInflater) context.getSystemService(Context.LAYOUT_INFLATER_SERVICE);
		inflater.inflate(R.layout.maze_camera_layout, this);
		
        m_CompassValueView = (TextView) findViewById(R.id.compass_values);
        m_OpenCvCameraView = (CameraBridgeViewBase) findViewById(R.id.opencv_view);
        m_OpenCvCameraView.setCvCameraViewListener(this);
        
        m_SensorManager = (SensorManager)getContext().getSystemService(Context.SENSOR_SERVICE);
        m_Gsensor = m_SensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        m_Msensor = m_SensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
	}
	
	public void onActivated()
    {
        OpenCVLoader.initAsync(OpenCVLoader.OPENCV_VERSION_2_4_7, getContext(), m_LoaderCallback);

        m_SensorManager.registerListener(this, m_Gsensor, SensorManager.SENSOR_DELAY_GAME);
        m_SensorManager.registerListener(this, m_Msensor, SensorManager.SENSOR_DELAY_GAME);
    }
	
	public void onDeactivated()
    {
        if (m_OpenCvCameraView != null)
            m_OpenCvCameraView.disableView();
        
        m_SensorManager.unregisterListener(this);
    }
}
